
#include "vision_service_direct_funcs.h"
#include "vision_service_direct_types.h"

#include <opencv2/opencv.hpp>

#include <unistd.h>
#include <fstream>
#include <iostream>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>

int main(int argc, char** argv) {
    printf("hello vision demo\n");

    int ret = 0;

    // 1. open msg receiver
    ret = vision_create_device_stream();  // 0: success; -1: failed
    if (ret) {
        printf("vision_create_device_stream failed.\n");
        return -1;
    }

    // 2. Get Stereo Images
    // DEVICE_STEREO_CAMERA_HEAD = 0,
    // DEVICE_STEREO_CAMERA_REAR,
    // DEVICE_STEREO_CAMERA_BOTTOM,
    // DEVICE_STEREO_CAMERA_RIGHT,
    // DEVICE_STEREO_CAMERA_LEFT,
    // DEVICE_STEREO_CAMERA_TOP,
    ImageFrame img1, img2;
    ret = vision_get_stereo_camera_distorted_frame(DeviceStream::DEVICE_STEREO_CAMERA_HEAD, MemoryType::MEMORY_VIRT,
                                                   &img1, &img2);
    printf("\nvision_get_stereo_camera_distorted_frame(DeviceStream::DEVICE_STEREO_CAMERA_HEAD,...):\n");
    if (!ret) {
        printf("img, rows: %d, cols: %d, step: %d, timestap: %lu\n", img1.rows, img1.cols, img1.step, img1.timestamp);
        cv::Mat img1_vrt(cv::Size2i(img1.cols, img1.rows), CV_8UC1, (uint8_t*)img1.data, img1.step);
        cv::Mat img2_vrt(cv::Size2i(img2.cols, img2.rows), CV_8UC1, (uint8_t*)img2.data, img2.step);
    }

    // 3. Get IMU Data
    AttFrame imu_data;
    char str_out[256], str_att[256];
    uint64_t time_now = vision_get_system_time(TimeSystem::TIME_FCS);
    // 3.1 Get One IMU
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_ATT, time_now, 50'000'000ULL, &imu_data);
    printf("\nvision_get_history_device_frame(DeviceStream::DEVICE_ATT...):\n");
    if (!ret) {
        // clang-format off
        sprintf(str_out,"%lu,%.4f,%.4f,%.4f,%.3f,%.3f,%.3f\n",
                imu_data.timestamp,
                imu_data.imu_gyro[0], imu_data.imu_gyro[1], imu_data.imu_gyro[2],
                imu_data.imu_accel[0], imu_data.imu_accel[1], imu_data.imu_accel[2]);

        sprintf(str_att,"%lu,%.4f,%.4f,%.4f,%.4f\n",
                imu_data.timestamp,
                imu_data.uav_quat[0], imu_data.uav_quat[1], imu_data.uav_quat[2], imu_data.uav_quat[3]);
        // clang-format on
        printf("%s", str_out);
    }

    // 3.2 Get IMUs
    uint64_t time_lst = time_now - 100'000'000ULL;
    int imu_num = 1024;
    AttFrame imu_datas[1024];
    ret = vision_get_history_device_frames(DeviceStream::DEVICE_ATT, time_lst, time_now, imu_datas, imu_num);
    printf("\nvision_get_history_device_frames(DeviceStream::DEVICE_ATT...):\n");
    printf("req_imu: imu0_last:tick_now = [%lu: %lu], ret: %d,  imu_num: %d \n", time_lst, time_now, ret, imu_num);
    if (!ret && imu_num) {
        for (int i = 0; i < imu_num; ++i) {
            // clang-format off
            sprintf(str_out,"%lu,%.4f,%.4f,%.4f,%.3f,%.3f,%.3f\n",
                    imu_datas[i].timestamp,
                    imu_datas[i].imu_gyro[0], imu_datas[i].imu_gyro[1], imu_datas[i].imu_gyro[2],
                    imu_datas[i].imu_accel[0], imu_datas[i].imu_accel[1], imu_datas[i].imu_accel[2]);

            sprintf(str_att,"%lu,%.4f,%.4f,%.4f,%.4f\n",
                    imu_datas[i].timestamp,
                    imu_datas[i].uav_quat[0], imu_datas[i].uav_quat[1], imu_datas[i].uav_quat[2], imu_datas[i].uav_quat[3]);
            // clang-format on
            printf("%s", str_out);
        }
    }

    // 4. Get position(NED)
    NedFrame ned_data;
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_NED, time_now, 50'000'000ULL, &ned_data);
    printf("\nvision_get_history_device_frame(DeviceStream::DEVICE_NED...):\n");
    if (!ret) {
        printf("%lu,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f \n", ned_data.timestamp, ned_data.coord[0], ned_data.coord[1],
               ned_data.coord[2], ned_data.speed[0], ned_data.speed[1], ned_data.speed[2]);
    }

    // 5. Get GPS/RTK
    GpsFrame gps_data;
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_GPS, time_now, 100'000'000ULL, &gps_data);
    printf("\nvision_get_history_device_frame(DeviceStream::DEVICE_GPS...):\n");
    if (!ret) {
    }

    // 下视 TOF: vision_get_history_device_frame(DeviceStream::DEVICE_SONAR,...)
    // ...

    // close stream
    vision_destroy_device_stream();

    return 0;
}